#include "bsp_key.h"
#include "gpio_config.h"
#include "stm32f4xx_hal.h"

/**
 * @brief Get the Joint Jog Key Raw Status object
 * 
 * @return jog_dir_en 点动按键指示的方向
 */
static jog_dir_en getJointJogKeyRawStatus(void)
{
    jog_dir_en jog_dir;
    //按键不按下时为高电平，按下时为低电平
    GPIO_PinState raw_key_forward = HAL_GPIO_ReadPin(JOG_FORWARD_GPIO_Port, JOG_FORWARD_Pin);   // 点动向前运动的按键
    GPIO_PinState raw_key_back = HAL_GPIO_ReadPin(JOG_BACK_GPIO_Port, JOG_BACK_Pin);            // 点动向后运动的按键

    raw_key_forward ^= 0x01;
    raw_key_back ^= 0x01;

    if (raw_key_forward && !raw_key_back)
    {
        jog_dir = JOG_FORWARD;  // 向前点动
    }
    else if (!raw_key_forward && raw_key_back)
    {
        jog_dir = JOG_BACKWARD; // 向后点动
    }
    else if (raw_key_forward && raw_key_back)
    {
        jog_dir = KEY_CALIB;    // 标定
    }
    else
    {
        jog_dir = JOG_STOP;     // 停止
    }
    return jog_dir;
}

/**
 * @brief Get the Pushrod Jog Key Raw Status object
 * 
 * @return jog_dir_en 点动按键指示的方向
 */
static jog_dir_en getPushrodJogKeyRawStatus(void)
{
    jog_dir_en jog_dir;
    GPIO_PinState raw_key_ext = HAL_GPIO_ReadPin(JOG_EXTEND_GPIO_Port, JOG_EXTEND_Pin);         // 点动伸长运动的按键
    GPIO_PinState raw_key_shrink = HAL_GPIO_ReadPin(JOG_SHRINK_GPIO_Port, JOG_SHRINK_Pin);      // 点动收缩运动的按键

    raw_key_ext ^= 0x01;
    raw_key_shrink ^= 0x01;

    if (raw_key_ext && !raw_key_shrink)
    {
        jog_dir = JOG_EXTEND;   // 点动伸长
    }
    else if (!raw_key_ext && raw_key_shrink)
    {
        jog_dir = JOG_SHRINK;   // 点动收缩
    }
    else
    {
        jog_dir = JOG_STOP;
    }
    return jog_dir;
}

/**
 * @brief 关节点动按键滤波
 * 
 * @return jog_dir_en 关节点动按键信号
 */
jog_dir_en jointJogKeyFilterSignal(void)
{
    const int16_t T_BOUND = 10;
    static jog_dir_en s_last_sta;
    static int16_t s_count = 0;
    static jog_dir_en s_jog_key_signal;

    jog_dir_en raw_sta = getJointJogKeyRawStatus();
    if (s_last_sta == raw_sta)
    {
        if (s_count++ >= T_BOUND)   // 按下信号持续一段时间才确认为有效
        {
            s_count = T_BOUND;
            s_jog_key_signal = raw_sta;
        }
    }
    else
    {
        // 状态变化时
        s_count = 0;
    }
    s_last_sta = raw_sta;  
    return s_jog_key_signal;
}


/**
 * @brief 推杆点动按键滤波
 * 
 * @return jog_dir_en 推杆点动按键信号
 */
jog_dir_en watch_pushrod_jog_key_dir;
jog_dir_en pushrodJogKeyFilterSignal(void)
{
    const int16_t T_BOUND = 10;
    static jog_dir_en s_last_sta;
    static int16_t s_count = 0;
    static jog_dir_en jog_key_signal;

    jog_dir_en raw_sta = getPushrodJogKeyRawStatus();
    watch_pushrod_jog_key_dir = raw_sta;
    if (s_last_sta == raw_sta)
    {
        if (s_count++ >= T_BOUND)
        {
            s_count = T_BOUND;
            jog_key_signal = raw_sta;
        }
    }
    else
    {
        // 状态变化时
        s_count = 0;
    }
    s_last_sta = raw_sta;  
    return jog_key_signal;
}
